#ifndef RADAR_H
#define RADAR_H
#include <boost/thread/thread.hpp>
#include <vector>
#include "StereoGrabber.h"
#include "MyBlob.h"
#include "CDatapoolServer.h"

class Radar
{
public:
	typedef enum
	{
		POWER_UP = 0,
		CALIBRATION,
		TRACK,
		SCAN
	}RadarMode_t;

private:

	double k1,k2,k3,k4;

	/* handle to the AIN2 rootfs */
	int m_PositionFd;

	/* handle to the AIN1 rootfs */
	int m_DistanceFd;

	/* stop running event handler */
	bool m_shallStop;

	/* loop thread handle */
	boost::thread * m_Thread;

	/* Vision grabber */
	StereoGrabber * m_Grabber;

	/**/
	int m_RadarPositionMap[255];

	/*----ROBOT-PRIVATE-VARS----*/
	/* References to Robot situation x value*/
	int &m_CurrentRobotX;

	/* References to Robot situation y value*/
	int &m_CurrentRobotY;

	/* References to Robot situation heading value*/
	double &m_CurrentHeading;

	bool GoAtMin;

	void IterationsThread();
	void MyRotate(CvPoint *ToRotate, CvPoint* RotationCenter, double RotationAngle);

	void PowerUpMode();
	void ScanMode();
	void CalibrationMode();

public:

	Radar(int &CurrentRobotX, int &CurrentRobotY, double &CurrentHeading);

	~Radar()
	{
		this->Stop();
		//delete m_Grabber;
		/*std::vector<MyBlob *>::iterator it = m_Obstacles.begin();
		while(it != m_Obstacles.end())
		{
			MyBlob * currentBlob = *it;
			it = m_Obstacles.erase(it);
			delete currentBlob;
		}*/
	}

	void Run();
	void Stop();

	int GetPosition();
	int GetDistance();

	/*----RADAR-PUBLIC-VARS----*/
	std::vector<MyBlob *> m_Obstacles;
	RadarMode_t m_RadarMode;

	/*----SCANNING-PUBLIC-VARS----*/
	int m_amplitudeBalayage;
	int m_headingBalayage;

	int cd_CenterScanCmdID;
	int cd_AmplituteScanCmdID;
	int cd_ServoOrientationCmdID;
	int cd_ServoOrientationPosID;
	int cd_ServoManualCtrlID;
};

#endif
